#include "pid.h"

struct PID pid;
int encoder = 0;

void stop(void) {
    TIM_SetCompare1(TIM2, 65535);
    TIM_SetCompare2(TIM2, 65535);
}

void pid_init(struct PID* pid) {
    pid->Kp = 5000;
    pid->Ki = 100;
    pid->Kd = 100;
    pid->target = 0;
    pid->err_last = 0;
    pid->integral = 0;
    pid->max_output = 50000;
    pid->max_integral = 10000;
}

void pid_reset(struct PID* pid) {
    pid->target = 0;
    pid->err_last = 0;
    pid->integral = 0;
}

int pid_count(struct PID* pid) {
    float err = 0;
    if (pid->target == 0) {
        stop();
        pid_reset(pid);
    }

    // 读取编码器并清零
    encoder = (int) ((int16_t) (TIM3->CNT));
    TIM3->CNT = 0;

    // 误差计算
    err = pid->target - encoder;

    // 积分限幅
    pid->integral += err;
    pid->integral = pid->integral > pid->max_integral ? pid->max_integral : pid->integral;
    pid->integral = pid->integral < -1 * pid->max_integral ? -1 * pid->max_integral : pid->integral;

    pid->output = pid->Kp * err +
                  pid->Ki * pid->integral +
                  pid->Kd * (err - pid->err_last);

    pid->err_last = err;

    pid->output = pid->output > pid->max_output ? pid->max_output : pid->output;
    pid->output = pid->output < -1 * pid->max_output ? -1 * pid->max_output : pid->output;

    return pid->output;
}

void set_motor(int output) {
    if (output > 0) {
        TIM_SetCompare1(TIM2, 0);
        TIM_SetCompare2(TIM2, output);
    } else {
        TIM_SetCompare1(TIM2, -1 * output);
        TIM_SetCompare2(TIM2, 0);
    }
}

